/*
 *   This file is part of ed_pvr.
 *
 *   ed_pvr is free software: you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation, either version 2 of the License, or
 *   (at your option) any later version.
 *
 *   ed_pvr is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with ed_pvr.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <inttypes.h>
#include <math.h>
#include <memory.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/socket.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <ctype.h>
#include <time.h>
#include <stdarg.h>

#include "mpegts.h"
#include "bitstream.h"
#include "utils.h"

#define MPEGTS_SECTION_LEN_PLUS_CRC		9
#define MPEGTS_SECTION_LEN_MAX			1023 // Maximum a 10 bit value can be

int mpegts_verbose_mode = 0;


//***************/

int8_t _mpegts_output_write_section(MPEGTS_SECTION *, uint8_t *, uint8_t *, uint8_t);
uint16_t _mpegts_pmt_add_program(MPEGTS_MAP_ES *, uint8_t *, uint16_t);
int8_t mpegts_write_pat(MPEGTS_NEWPAT_STRUCT *);
int8_t mpegts_write_pmt(MPEGTS_NEWPMT_STRUCT *);
void mpegts_output_writepts(uint8_t *, uint64_t, uint8_t);
void (*mpegts_printf_redir)(void *, const char *format, va_list ap);


//***************/

void mpegts_debug_printf(void * extra, const char *format, ...)
{
	if (mpegts_printf_redir)
	{
		va_list ap;
		va_start(ap, format);
		printf(format, ap);
//		(*mpegts_printf_redir)(extra, format, ap);
		va_end(ap);
	}
}

MPEGTS_PACKET * mpegts_parse_packet(BITSTREAM *bitstream)
{
	MPEGTS_PACKET * ret = calloc(1, sizeof(MPEGTS_PACKET));

	if (ret)
	{
		int8_t rc = 0;
		ret->type = -1;
		ret->sync_byte = bits_get_bit_count(bitstream, 8, &rc);
		if (ret->sync_byte != TS_SYNC_BYTE)
		{
			mpegts_debug_printf(NULL, "[MPEGTS] No Sync Byte, invalid MPEG-TS\n");
			free(ret);
			return NULL;
		}

		ret->ts_error = bits_get_next_bit(bitstream);
		if (ret->ts_error)
		{
			if (mpegts_verbose_mode >= 2)
				mpegts_debug_printf(NULL, "[MPEGTS] TS_ERROR is set, bad packet\n");
			free(ret);
			return NULL;
		}

		ret->payload_start = bits_get_next_bit(bitstream);
		ret->ts_priority = bits_get_next_bit(bitstream);
		ret->pid = (uint16_t) bits_get_bit_count(bitstream, 13, &rc);
		ret->ts_scrambling = (uint8_t) bits_get_bit_count(bitstream, 2, &rc);
		ret->adapt_field_control = (uint8_t) bits_get_bit_count(bitstream, 2, &rc);
		ret->cont_counter = (uint8_t) bits_get_bit_count(bitstream, 4, &rc);


		ret->packet_data = bitstream->stream;
		ret->packet_data_size = bitstream->len;

		if (mpegts_verbose_mode >= 1)
		{
			mpegts_debug_printf(NULL, "[MPEGTS] PID: %d (%x)\n", ret->pid, ret->pid);
			mpegts_debug_printf(NULL, "[MPEGTS] payload_start: %d, cont_count: %d\n", ret->payload_start, ret->cont_counter);
		}
	}
	return ret;
}

MPEGTS_PACKET * mpegts_parse_packet2(BITSTREAM *bitstream, MPEGTS_PARSE_STRUCT *tsparser)
{
	MPEGTS_PACKET *packet = mpegts_parse_packet(bitstream);

	if (packet)
	{
		if (packet->adapt_field_control ==  ADAPT_FIELD_ONLY_NO_PAYLOAD || packet->adapt_field_control ==  ADAPT_FIELD_AND_PAYLOAD )
		{
//			mpegts_debug_printf(NULL, "parsing af...\n");
			packet->adaptation_field = mpegts_parse_adaptation_field(bitstream);
		}

		if (packet->payload_start)
		{
//			printf("mpegts_parse_packet2: 0x%x\n", packet->pid);
//			uint16_t pmt_pid = 0;
//			printf("packet2: 0x%x -- %p (pmtpid: 0x%x)\n", packet->pid, tsparser, tsparser->pmt_pid);
//			mpegts_debug_printf(NULL, "HMM: 0x%x 0x%x 0x%x 0x%x\n", bitstream->stream[(bitstream->bit_offset/8)], bitstream->stream[(bitstream->bit_offset/8)+1], bitstream->stream[(bitstream->bit_offset/8)+2], bitstream->stream[(bitstream->bit_offset/8)+3]);
			if (bitstream->stream[bitstream->bit_offset/8] == 0x00)
			{
//				if (packet->pid == 0 && tsparser && !tsparser->pmt_pid)
				if (packet->pid == 0)
				{
					packet->type = TID_PAT;

			 		if (tsparser)
					{
						tsparser->stream_type = packet->type;
						tsparser->pmt_type = 0;

						tsparser->pat = mpegts_parse_pat(bitstream);

						if (tsparser->pat && tsparser->pat->first_program)
							tsparser->program_number = tsparser->pat->first_program->program_number;

//						printf("PAT PARse: %d, %d\n", tsparser->program_number, tsparser->pmt_pid);
						if (tsparser->pat && tsparser->program_number && !tsparser->pmt_pid)
						{
							MPEGTS_PAT_PROGRAM_STRUCT *t = tsparser->pat->first_program;

							while (t)
							{
								if (t->program_number == tsparser->program_number)
								{
									tsparser->pmt_pid = t->pid;
									printf("[MPEGTS] PAT, program_number: %d (0x%x) pid: (%d) 0x%x\n", t->program_number,t->program_number, t->pid, t->pid);
									break;
								}
								t = t->next;
							}
						}
					}
				}
				else if (tsparser && tsparser->pmt_pid && packet->pid == tsparser->pmt_pid)
				{
					packet->type = TID_PMT;
					tsparser->stream_type = packet->type;
					tsparser->pmt_type = 0;

					if (tsparser->pmt)
						mpegts_free_pmt(tsparser->pmt);

					tsparser->pmt = mpegts_parse_pmt(bitstream, packet->payload_start);
					//mpegts_print_pmt(tsparser->pmt);
//					printf("parsed pmt: %d (0x%x) -- null? %p\n", tsparser->pmt_pid, tsparser->pmt_pid, tsparser->pmt);
				}
				else if (bitstream->stream[(bitstream->bit_offset/8)+1] == 0x00 && bitstream->stream[(bitstream->bit_offset/8)+2] == 0x01)
				{
					packet->type = bitstream->stream[(bitstream->bit_offset/8)+3] & 0xFF;

					if (tsparser)
					{
						tsparser->stream_type = packet->type;
						tsparser->pmt_type = 0;

						if (tsparser->pmt)
						{
							MPEGTS_MAP_ES *es = tsparser->pmt->first_es;

							while(es)
							{
								if (es->pid == packet->pid)
								{
//									printf("\tEs, PID: %d, type: 0x%x\n", packet->pid, es->stream_type);
									break;
								}
								es = es->next;
							}

							es = tsparser->pmt->first_es;
							while(es)
							{
								if (es->pid == packet->pid)
								{

									tsparser->pmt_type = es->stream_type;
									break;
								}
								es = es->next;
							}
						}
//						printf("PType: 0x%x, PMTType: 0x%x\n", packet->type, pmt_type);
					}
					packet->payload_start_struct = mpegts_parse_payload_start(bitstream);
				}
				else if (bitstream->stream[(bitstream->bit_offset/8)+1] == TID_PMT)
				{
					packet->type = TID_PMT;
				}
				else
				{
				}
			}
			else
			{
//				printf("BITSTReAM NOT zero\n");
			}
		}
	}
	return packet;
}


void mpegts_print_adaptation_field(MPEGTS_ADAPTATION_FIELD *af)
{
	if (af)
	{
		mpegts_debug_printf(NULL, "[MPEGTS] Printing Adaptation Field, Len: %d\n", af->adaptation_field_length);
		mpegts_debug_printf(NULL, "[MPEGTS]  ->discontinuity: %d\n", af->discontinuity);
		mpegts_debug_printf(NULL, "[MPEGTS]  ->random_access: %d\n", af->random_access);
		mpegts_debug_printf(NULL, "[MPEGTS]  ->es_prio: %d\n", af->es_prio);
		mpegts_debug_printf(NULL, "[MPEGTS]  ->pcr_flag: %d\n", af->pcr_flag);
		if (af->pcr_flag)
		{
			mpegts_debug_printf(NULL, "[MPEGTS]     ->pcr_base: %" PRId64 "\n", af->pcr_base);
			mpegts_debug_printf(NULL, "[MPEGTS]     ->pcr_reserved: %d\n", af->pcr_reserved);
			mpegts_debug_printf(NULL, "[MPEGTS]     ->pcr_ext: %d\n", af->pcr_ext);
		}
		mpegts_debug_printf(NULL, "[MPEGTS]  ->opcr_flag: %d\n", af->opcr_flag);
		mpegts_debug_printf(NULL, "[MPEGTS]  ->splice_flag: %d\n", af->splice_flag);
		mpegts_debug_printf(NULL, "[MPEGTS]  ->ts_private_flag: %d\n", af->ts_private_flag);
		mpegts_debug_printf(NULL, "[MPEGTS]  ->field_ext_flag: %d\n", af->field_ext_flag);
		mpegts_debug_printf(NULL, "[MPEGTS]  ->random_access: %d\n", af->random_access);

	}
}

MPEGTS_ADAPTATION_FIELD * mpegts_parse_adaptation_field(BITSTREAM *bitstream)
{
	MPEGTS_ADAPTATION_FIELD *tmp = NULL;

	if (bitstream)
	{
		tmp = calloc(1, sizeof(MPEGTS_ADAPTATION_FIELD));
		if (tmp)
		{
			int8_t rc = 0;

//			memset(tmp, 0, sizeof(MPEGTS_ADAPTATION_FIELD));

			tmp->adaptation_field_length = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
//			mpegts_debug_printf(NULL, "----- AF LEN: %d\n", tmp->adaptation_field_length);
			if (tmp->adaptation_field_length > 0)
			{
				uint32_t start = bitstream->bit_offset;

				tmp->discontinuity = bits_get_next_bit(bitstream);
				tmp->random_access = bits_get_next_bit(bitstream);
				tmp->es_prio = bits_get_next_bit(bitstream);
				tmp->pcr_flag = bits_get_next_bit(bitstream);
				tmp->opcr_flag = bits_get_next_bit(bitstream);
				tmp->splice_flag = bits_get_next_bit(bitstream);
				tmp->ts_private_flag = bits_get_next_bit(bitstream);
				tmp->field_ext_flag = bits_get_next_bit(bitstream);

				if (tmp->random_access && mpegts_verbose_mode >= 1)
					mpegts_debug_printf(NULL, "GOP\n");

				if (tmp->pcr_flag)
				{
					tmp->pcr_base = (uint64_t) bits_get_bit_count(bitstream, 33, &rc);
					tmp->pcr_reserved = (uint8_t) bits_get_bit_count(bitstream, 6, &rc);
					tmp->pcr_ext = (uint16_t) bits_get_bit_count(bitstream, 9, &rc);
				}

				if (tmp->opcr_flag)
				{
					tmp->opcr_base = (uint64_t) bits_get_bit_count(bitstream, 33, &rc);
					tmp->opcr_reserved = (uint8_t) bits_get_bit_count(bitstream, 6, &rc);
					tmp->opcr_ext = (uint16_t) bits_get_bit_count(bitstream, 9, &rc);
				}

				if (tmp->splice_flag)
					bits_get_bit_count(bitstream, 8, &rc);

				if (tmp->ts_private_flag)
				{
					tmp->transport_private_data_length = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);

					if (tmp->transport_private_data_length > 0)
					{
						bits_get_bit_count(bitstream, BYTE_SIZE * tmp->transport_private_data_length, &rc);
					}
				}

				if (tmp->field_ext_flag )
				{
					mpegts_debug_printf(NULL, "[MPEGTS] ERROR! We don't implement the adaptation field ext parsing, this is broke\n");
				}
				if ((bitstream->bit_offset - start) % BYTE_SIZE != 0)
				{
					mpegts_debug_printf(NULL, "[MPEGTS] BIG PROBLEMS!\n");
					return NULL;
				}
				else
				{
					tmp->stuffing_bytes = tmp->adaptation_field_length - ((bitstream->bit_offset - start) / 8);

					if (tmp->stuffing_bytes)
					{
						int q;
						uint8_t skip;

						for (q = 0; q < tmp->stuffing_bytes; q++)
						{
							skip = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
							if (skip != MPEGTS_STUFF_BYTE)
							{
//								mpegts_debug_printf(NULL, "[MPEGTS] Error skipping stuffing byte: %x\n", skip);
								//return NULL;
							}
						}
					}
				}
			}
		}
	}
	return tmp;
}

void mpegts_print_pat(MPEGTS_PAT *pat)
{
	if (pat)
	{
		MPEGTS_PAT_PROGRAM_STRUCT *t =  pat->first_program;
		uint8_t program_count = 1;
		mpegts_debug_printf(NULL, "[MPEGTS] Printing PAT..\n");
		mpegts_debug_printf(NULL, "[MPEGTS]   ->table_id: %d\n", pat->table_id);
		mpegts_debug_printf(NULL, "[MPEGTS]   ->section_syntax_indicator: %d\n", pat->section_syntax_indicator);
		mpegts_debug_printf(NULL, "[MPEGTS]   ->zero: %d\n", pat->zero);
//        mpegts_debug_printf(NULL, "[MPEGTS]   ->reserved1 (should be 3): %d\n", pat->reserved1);
		mpegts_debug_printf(NULL, "[MPEGTS]   ->section_length: %d\n", pat->section_length);
		mpegts_debug_printf(NULL, "[MPEGTS]   ->transport_stream_id: %d\n", pat->transport_stream_id);
//        mpegts_debug_printf(NULL, "[MPEGTS]   ->reserved2 (should be 3): %d\n", pat->reserved2);
		mpegts_debug_printf(NULL, "[MPEGTS]   ->version_number: %d\n", pat->version_number);
		mpegts_debug_printf(NULL, "[MPEGTS]   ->current_next_indicator: %d\n", pat->current_next_indicator);
		mpegts_debug_printf(NULL, "[MPEGTS]   ->section_number: %d\n", pat->section_number);
		mpegts_debug_printf(NULL, "[MPEGTS]   ->last_section_number: %d\n", pat->last_section_number);
		mpegts_debug_printf(NULL, "[MPEGTS]   There are %d program(s) in the PAT\n", pat->program_count);

		while (t)
		{
			mpegts_debug_printf(NULL, "[MPEGTS]   (%d) ->program_number/service_id: %d\n", program_count, t->program_number);
//            mpegts_debug_printf(NULL, "[MPEGTS]   (%d) ->reserved: %d\n", program_count, t->reserved);
			if (t->program_number)
				mpegts_debug_printf(NULL, "[MPEGTS]   (%d) ->pid: %d (0x%X) -- PMT PID\n", program_count, t->pid, t->pid);
			else
				mpegts_debug_printf(NULL, "[MPEGTS]   (%d) ->pid: %d (0x%X) -- NIT PID\n", program_count, t->pid, t->pid);

			t = t->next;
			program_count++;
		}
	}
}

MPEGTS_PAT * mpegts_parse_pat(BITSTREAM *bitstream)
{
//	printf("PARSE PAT!\n");
	MPEGTS_PAT * ret = NULL;
	if (bitstream)
	{
		int8_t rc = 0;
		uint8_t pointer = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
		if (pointer != 0)
			mpegts_debug_printf(NULL, "POINTER: %d\n", pointer);
		if (pointer == 0)
		{
			int i;
			ret = calloc(1, sizeof(MPEGTS_PAT));

			if (ret)
			{
//				memset((void*)ret, 0, sizeof(MPEGTS_PAT));

				ret->table_id = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
				ret->section_syntax_indicator = bits_get_next_bit(bitstream);
				ret->zero = bits_get_next_bit(bitstream);
				ret->reserved1 =  (uint8_t) bits_get_bit_count(bitstream, 2, &rc);
				ret->section_length =  (uint16_t) bits_get_bit_count(bitstream, 12, &rc);
				ret->transport_stream_id =  (uint16_t) bits_get_bit_count(bitstream, 16, &rc);
				ret->reserved2 = (uint8_t) bits_get_bit_count(bitstream, 2, &rc);
				ret->version_number = (uint8_t) bits_get_bit_count(bitstream, 5, &rc);
				ret->current_next_indicator = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
				ret->section_number = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
				ret->last_section_number = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);

//				printf("pat, sec_len: %d - %d\n", ret->section_length, ret->section_length - PAT_BYTES_AFTER_SECTION_LEN - CRC_LENGTH);
				for (i = 0; i < (ret->section_length - PAT_BYTES_AFTER_SECTION_LEN - CRC_LENGTH) / PAT_PROGRAM_LENGTH; i++)
				{
					MPEGTS_PAT_PROGRAM_STRUCT *t = calloc(1, sizeof(MPEGTS_PAT_PROGRAM_STRUCT));
//					memset((void*)t, 0, sizeof(MPEGTS_PAT_PROGRAM_STRUCT));
					t->program_number = (uint16_t) bits_get_bit_count(bitstream, 16, &rc);
					t->reserved = (uint8_t) bits_get_bit_count(bitstream, 3, &rc);

					t->pid = (uint16_t) bits_get_bit_count(bitstream, 13, &rc);

					if (!ret->first_program)
					{
						ret->first_program = ret->current_program = t;
					}
					else
					{
						ret->current_program->next = t;
						ret->current_program = ret->current_program->next;
					}
					ret->program_count++;
//					printf("[MPEGTS] PAT, Adding Program/Network PID: %d (0x%X) -- PGM_NUM: 0x%x -- Count: %d\n", t->pid, t->pid, t->program_number, ret->program_count);
				}
			}
		}
		else
			mpegts_debug_printf(NULL, "[MPEGTS] We encountered a PAT PID, but the pointer field is non-zero.. it's bad\n");
	}
	return ret;
}

int8_t _mpegts_output_write_section(MPEGTS_SECTION *sec, uint8_t *data, uint8_t *ts_header, uint8_t ts_header_len)
{
	if (sec && sec->section_data)
	{
		uint16_t section_len = MPEGTS_SECTION_LEN_PLUS_CRC + sec->section_len;
		uint8_t packet[MPEGTS_SECTION_LEN_MAX + section_len + 12];
		uint32_t i = 0, j, crc = 0;

		if (section_len > MPEGTS_SECTION_LEN_MAX)
		{
			mpegts_debug_printf(NULL, "[MPEGTS] Section Length is too big: %d, Max: %d\n", section_len, MPEGTS_SECTION_LEN_MAX);
			return -1;
		}

		packet[i++] = sec->table_id;
		packet[i++] = 0xB0 | (section_len >> 8); // 0xB0 = 1 0 11 00 00 in binary
		packet[i++] = section_len & 0xFF;        // Rest of the section length
		packet[i++] = sec->ts_id >> 8;           // PID: PAT = 0, PMT = Variable
		packet[i++] = sec->ts_id & 0xFF;

//		tmp = sec->version_number << 1;
//		tmp1 = 0xC1 | tmp;
		packet[i++] = 0xC1 | (sec->version_number << 1);                      // 0xC1 = 11 00000 1 in binary
//		mpegts_debug_printf(NULL, "HMM: %x\n",  packet[i-1]);
		packet[i++] = 0;                         // section  number
		packet[i++] = 0;                         // last section number

		for (j = 0; j < sec->section_len; j++)
			packet[i++] = sec->section_data[j];

		crc = utils_crc32(packet, i);

		packet[i++] = (crc >> 24) & 0xFF;
		packet[i++] = (crc >> 16) & 0xFF;
		packet[i++] = (crc >> 8) & 0xFF;
		packet[i++] = crc & 0xFF;

		if (data)
		{
//			int j = 0;
			memset(data, 0xFF, TS_PACKET_SIZE);
			if (ts_header && ts_header_len)
				memcpy(data, ts_header, ts_header_len);
			memcpy(data + ts_header_len, packet, i);
		}
//		mpegts_output_packetize(program, packet, i, 1, 0, 0, 0);
	}
	return 0;
}

uint16_t _mpegts_pmt_add_program(MPEGTS_MAP_ES * program, uint8_t *packet, uint16_t i)
{
	int k;

	packet[i++] = program->stream_type;

	packet[i++] = 0xE0 | ((program->pid >> 8) & 0x1F);
	packet[i++] = (program->pid & 0xFF);

	packet[i++] = 0xF0 | (program->info_len >> 8);
	packet[i++] = program->info_len & 0xFF;
	for (k = 0; k < program->info_len; k++)
	{
		packet[i++] = program->descriptor[k];
	}
	return i;
}

int8_t mpegts_write_pmt(MPEGTS_NEWPMT_STRUCT *pmt)
{
	int8_t ret = 1;
	if (pmt)
	{
		uint8_t packet[MPEGTS_SECTION_LEN_MAX + TS_PACKET_SIZE] = {0};
		uint16_t i = 0, j = 0;
		MPEGTS_SECTION section = {0};
		MPEGTS_MAP_ES * tmp_es = pmt->desc_es;

		section.table_id = TID_PMT;
		section.ts_id = pmt->program_number;
//		mpegts_debug_printf(NULL, "PMT Program Num: %d\n", pmt->program_number);
		//PCR
		packet[i++] = 0xE0 | ((pmt->pcr_pid >> 8) & 0x1F);
		packet[i++] = pmt->pcr_pid & 0xFF;

		//PMT Descriptor, there shouldn't be one (len = 0)
		packet[i++] = 0xF0 | (pmt->program_descriptor_len >> 8);
		packet[i++] = pmt->program_descriptor_len & 0xFF;
//		mpegts_debug_printf(NULL, "desc len: %d\n", pmt->program_descriptor_len);
		if (pmt->program_descriptor_len)
		{
			for (j = 0; j < pmt->program_descriptor_len; j++)
				packet[i++] =pmt->program_descriptor[j];
		}

		while (tmp_es)
		{
//			mpegts_debug_printf(NULL, "Adding, PID: %d\n", tmp_es->pid);
			i = _mpegts_pmt_add_program(tmp_es, packet, i);
			tmp_es = tmp_es->next;
		}

		section.section_data = packet;
		section.section_len = i;

		_mpegts_output_write_section(&section, pmt->new_pmt_data, pmt->ts_header, pmt->ts_header_len);

		ret = 0;
	}
	return ret;
}

int8_t mpegts_write_pat(MPEGTS_NEWPAT_STRUCT *pat)
{
	int8_t ret = 1;

	if (pat)
	{
		uint8_t packet[4] = {0};
		uint16_t i = 0;
		MPEGTS_SECTION section = {0};

		section.table_id = TID_PAT;
		section.ts_id = pat->ts_id;
		section.version_number = pat->version;

//		mpegts_debug_printf(NULL, "Program Num: %d, PMT_PID: %d\n", pat->program_num, pat->pmt_pid);
		packet[i++] = pat->program_num >> 8;
 		packet[i++] = pat->program_num & 0xFF;

		packet[i++] = 0xE0 | ((pat->pmt_pid >> 8) & 0x1F);
		packet[i++] = pat->pmt_pid & 0xFF;

		section.section_data = packet;
		section.section_len = i;

		_mpegts_output_write_section(&section, pat->new_pat_data, pat->ts_header, pat->ts_header_len);

		ret = 0;
	}
	return ret;
}

void mpegts_free_packet(MPEGTS_PACKET *p)
{
	if (p)
	{
		if (p->adaptation_field)
			free(p->adaptation_field);

		if (p->payload_start_struct)
			free(p->payload_start_struct);

//		if (p->pat)
//			mpegts_free_pat(p->pat);

//		if (p->pmt)
//			mpegts_free_pmt(p->pmt);

		free(p);
	}
}


MPEGTS_PAT *mpegts_dup_pat(MPEGTS_PAT *pat)
{
	MPEGTS_PAT * ret = NULL;
	if (pat)
	{
		ret = calloc(1, sizeof(MPEGTS_PAT));
		if (ret)
		{
			MPEGTS_PAT_PROGRAM_STRUCT *t = pat->first_program;
			while (t)
			{
				MPEGTS_PAT_PROGRAM_STRUCT * pat_pgm = calloc(1, sizeof(MPEGTS_PAT_PROGRAM_STRUCT));
				if (pat_pgm)
				{
					memcpy(pat_pgm, t, sizeof(MPEGTS_PAT_PROGRAM_STRUCT));

					if (!ret->first_program)
					{
						ret->first_program = ret->current_program = pat_pgm;
					}
					else
					{
						ret->current_program->next = pat_pgm;
						ret->current_program = pat_pgm;
					}
				}

				t = t->next;
			}
		}
	}

	return ret;
}
void mpegts_free_pat(MPEGTS_PAT *pat)
{
	if (pat)
	{
		MPEGTS_PAT_PROGRAM_STRUCT *t = pat->first_program;
		MPEGTS_PAT_PROGRAM_STRUCT *del = NULL;
		while (t)
		{
			del = t;
			t = t->next;
			free(del);
		}
		free(pat);
	}
}

void mpegts_free_pmt(MPEGTS_PMT * pmt)
{
	if (pmt)
	{
		MPEGTS_MAP_ES * t = pmt->first_es;
		MPEGTS_MAP_ES * del = NULL;

		while (t)
		{
			del = t;
			t = t->next;
			free(del);
		}

		free(pmt);
	}

	return;
}

void mpegts_print_pmt(MPEGTS_PMT *pmt)
{
	if (pmt)
	{
		int i;
		MPEGTS_MAP_ES * t = pmt->first_es;
		uint8_t program_count = 1;

		printf( "[MPEGTS] Printing PMT..\n");
		printf( "[MPEGTS]   ->table_id: %d\n", pmt->table_id);
		printf( "[MPEGTS]   ->section_syntax_indicator: %d\n", pmt->section_syntax_indicator);
		printf( "[MPEGTS]   ->zero: %d\n", pmt->zero);

		printf( "[MPEGTS]   ->section_length: %d\n", pmt->section_length);
		printf( "[MPEGTS]   ->program_number: %d\n", pmt->program_number);
		printf( "[MPEGTS]   ->version_number: %d\n", pmt->version_number);
		printf( "[MPEGTS]   ->current_next_indicator: %d\n", pmt->current_next_indicator);
		printf( "[MPEGTS]   ->section_number: %d\n", pmt->section_number);
		printf( "[MPEGTS]   ->last_section_number: %d\n", pmt->last_section_number);
		printf( "[MPEGTS]   ->pcr_pid: %d\n", pmt->pcr_pid);
		printf( "[MPEGTS]   ->program_info_length: %d\n", pmt->program_info_length);
		printf( "[MPEGTS]   There are %d program(s) in the PMT\n", pmt->es_count);

		while (t)
		{
			printf( "[MPEGTS]   (%d) ->stream_type: %d (0x%X)\n", program_count, t->stream_type, t->stream_type);
			printf( "[MPEGTS]   (%d) ->pid: %d (0x%X)\n", program_count, t->pid, t->pid);
			printf( "[MPEGTS]   (%d) ->info_len: %d\n", program_count, t->info_len);
			for(i=0; i < t->info_len; i++)
				if (isprint(t->descriptor[i]))
					printf( "%c", t->descriptor[i]);
				else
					printf( ".");
			printf( "\n");
			t = t->next;
			program_count++;
		}

	}
}

MPEGTS_PMT * mpegts_parse_pmt(BITSTREAM *bitstream, uint8_t payload_start)
{
	MPEGTS_PMT * pmt = NULL;

	if (bitstream)
	{
		int8_t rc = 0;
		uint8_t pointer = 0;
		int32_t tmp_len = 0;

		if (payload_start)
		{
			pointer = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
//            mpegts_debug_printf(NULL, "Pointer Field: %d\n", pointer);
			if (pointer)
				bits_get_bit_count(bitstream, pointer * 8, &rc);
		}


//		pmt = calloc(1, sizeof(MPEGTS_PMT));
		pmt = malloc(sizeof(MPEGTS_PMT));

		if (pmt)
		{
			int loop = 0;
			int k;

			while(bitstream->bit_offset/8 < bitstream->len)
			{
				loop++;
				memset(pmt, 0, sizeof(MPEGTS_PMT));

				pmt->table_id = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
//				printf("TableID: 0x%x\n", pmt->table_id);
/*
				printf("\n\n");
				for (k=0; k < bitstream->len; k++)
					printf("0x%02X ", bitstream->stream[k]);
				printf("\n\n");
*/
				pmt->section_syntax_indicator = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);;
				pmt->zero = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
				pmt->reserved1 = (uint8_t) bits_get_bit_count(bitstream, 2, &rc);
				tmp_len = pmt->section_length = (uint16_t) bits_get_bit_count(bitstream, 12, &rc);
				pmt->program_number = (uint16_t) bits_get_bit_count(bitstream, 16, &rc);
				pmt->reserved2 = (uint8_t) bits_get_bit_count(bitstream, 2, &rc);
				pmt->version_number = (uint8_t) bits_get_bit_count(bitstream, 5, &rc);

				pmt->current_next_indicator = bits_get_next_bit(bitstream);
				pmt->section_number = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
				pmt->last_section_number = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
				pmt->reserved3 = (uint8_t) bits_get_bit_count(bitstream, 3, &rc);
				pmt->pcr_pid = (uint16_t) bits_get_bit_count(bitstream, 13, &rc);
				pmt->reserved4 = (uint8_t) bits_get_bit_count(bitstream, 4, &rc);
				pmt->program_info_length = (uint16_t) bits_get_bit_count(bitstream, 12, &rc);

//				if (mpegts_verbose_mode >= 1)
//				printf("[TS] PMT, PCR PID: %d\n", pmt->pcr_pid);

//				mpegts_print_pmt(pmt);
//				printf("pmt->section_length: %d\n", pmt->section_length);

				if (pmt->program_info_length)
				{
					pmt->program_info_data = calloc(1, pmt->program_info_length);
					for (k = 0; k < pmt->program_info_length; k++)
						pmt->program_info_data[k] = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
				}


//				printf("PMT, tmp_len2: %d (%d)\n", tmp_len, PMT_CONST_LENGTH);
				if (pmt->table_id == TID_PMT)
				{
//					printf("TID IS GOOD: \n");
					tmp_len -= (pmt->program_info_length + CRC_LENGTH + PMT_BYTES_AFTER_SECTION_LEN); // Bytes left to process which are es_info's
					while (tmp_len > 0)
					{
						MPEGTS_MAP_ES * es_info = calloc(1, sizeof(MPEGTS_MAP_ES));
//						mpegts_debug_printf(NULL, "PMT, tmp_len1: %d\n", tmp_len);

						es_info->stream_type = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
						es_info->reserved1 =  (uint8_t) bits_get_bit_count(bitstream, 3, &rc);
						es_info->pid = (uint16_t) bits_get_bit_count(bitstream, 13, &rc);
						es_info->reserved1 = (uint8_t) bits_get_bit_count(bitstream, 4, &rc);
						es_info->info_len = (uint16_t) bits_get_bit_count(bitstream, 12, &rc);

//						printf("PMT, stream_type: 0x%x (%d), pid: 0x%x (%d), info_len: %d\n",  es_info->stream_type, es_info->stream_type, es_info->pid, es_info->pid, es_info->info_len);
						if (es_info->info_len)
						{
							int i;
							if (es_info->info_len > tmp_len)
							{
								mpegts_debug_printf(NULL, "BIG FREAKING ERROR!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
								free(es_info);
								break;
							}
							es_info->descriptor = (uint8_t*) calloc(1, es_info->info_len);
							for (i = 0; i < es_info->info_len; i++)
								es_info->descriptor[i] = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
						}

						tmp_len -= (PMT_PROGRAM_LENGTH + es_info->info_len);
//	    	            mpegts_debug_printf(NULL, "PMT, tmp_len1: %d\n", tmp_len);

						if (pmt->first_es == NULL)
						{
							pmt->current_es = pmt->first_es = es_info;
						}
						else
						{
							pmt->current_es->next = es_info;
							pmt->current_es = pmt->current_es->next;
						}
						pmt->es_count++;
					}

//					printf("[MPEGTS] PMT Loop Count: %d\n", loop);
//					mpegts_print_pmt(pmt);
					return pmt;
				}
				else
				{
					tmp_len -= (pmt->program_info_length + PMT_BYTES_AFTER_SECTION_LEN); // Bytes left to process which are es_info's
//					printf("PMT TableID is bad, skiping : %d\n", tmp_len);
					for (k = 0; k < tmp_len; k++)
						bits_get_bit_count(bitstream, 8, &rc);
				}
			}
		}
	}
/*
	if (pmt)
	{
		free(pmt);
		pmt = NULL;
	}
*/
	return pmt;
}

uint64_t mpegts_parse_pts(BITSTREAM *bitstream)
{
    uint64_t pts_tmp = 0;
    uint64_t ret = 0;
    int8_t rc = 0;

    pts_tmp = (uint64_t) bits_get_bit_count(bitstream, 3, &rc);

    if (bits_get_next_bit(bitstream) != 1)
        mpegts_debug_printf(NULL, "[TS] PTS or DTS ERROR! First Marker is not 1\n");

    ret = (pts_tmp << 30);

    pts_tmp = (uint64_t) bits_get_bit_count(bitstream, 15, &rc);

    if (bits_get_next_bit(bitstream) != 1)
        mpegts_debug_printf(NULL, "[TS] PTS_ONLY ERROR! Second Marker is not 1\n");

    ret |= (pts_tmp << 15);

    pts_tmp = (uint64_t) bits_get_bit_count(bitstream, 15, &rc);

    if (bits_get_next_bit(bitstream) != 1)
        mpegts_debug_printf(NULL, "[TS] PTS_ONLY ERROR! Third Marker is not 1\n");

    ret |= (pts_tmp);

    return ret;
}

MPEGTS_PAYLOAD_START * mpegts_parse_payload_start(BITSTREAM *bitstream)
{
    MPEGTS_PAYLOAD_START *p = NULL;
    if (bitstream)
    {
        p = calloc(1, sizeof(MPEGTS_PAYLOAD_START));
        if (p)
        {
            int8_t rc = 0;
            uint8_t start = 0;
//			uint32_t _offset = 0;
//			memset(p, 0, sizeof(MPEGTS_PAYLOAD_START));
/*
			mpegts_debug_printf(NULL, "[0x%x 0x%x 0x%x 0x%x]\n", bitstream->stream[bitstream->bit_offset/8],
												bitstream->stream[(bitstream->bit_offset/8)+1],
												bitstream->stream[(bitstream->bit_offset/8)+2],
												bitstream->stream[(bitstream->bit_offset/8)+3]);
*/
            p->start_code = (uint32_t) bits_get_bit_count(bitstream, 24, &rc);
            if (p->start_code != 0x1)
            {
                free(p);
                return NULL;
            }

            p->stream_id = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);
            p->len = (uint16_t) bits_get_bit_count(bitstream, 16, &rc);
            p->one_zero = (uint8_t) bits_get_bit_count(bitstream, 2, &rc);
            p->pes_scrambling = (uint8_t) bits_get_bit_count(bitstream, 2, &rc);
            p->pes_priority = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->data_alignment = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->copyright = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->original = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->pts_dts_flag = (uint8_t) bits_get_bit_count(bitstream, 2, &rc);
            p->escr_flag = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->es_rate_flag = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->dsm_trick_mode_flag = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->additional_copy_info_flag = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->pes_crc_flag = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->pes_extension_flag = (uint8_t) bits_get_bit_count(bitstream, 1, &rc);
            p->pes_header_data_len = (uint8_t) bits_get_bit_count(bitstream, 8, &rc);

//            mpegts_debug_printf(NULL, "pes_len: %d, pes_header_len: %d\n", p->len, p->pes_header_data_len);

            start  = bitstream->bit_offset / BYTE_SIZE;

            switch (p->pts_dts_flag)
            {
                case PTS_ONLY :
                    // '10', next 4 bits should be '0010'
                    if (bits_get_next_bit(bitstream) == 0 && bits_get_next_bit(bitstream) == 0 && bits_get_next_bit(bitstream) == 1 && bits_get_next_bit(bitstream) == 0 )
                        p->pts = mpegts_parse_pts(bitstream);
                    else
                        mpegts_debug_printf(NULL, "[TS] PTS_ONLY - ERROR - PTS header not correct\n");
                    break;

                case PTS_AND_DTS :
                    // '11', next 4 bits should be '0011'
                    if (bits_get_next_bit(bitstream) == 0 && bits_get_next_bit(bitstream) == 0 && bits_get_next_bit(bitstream) == 1 && bits_get_next_bit(bitstream) == 1 )
                        p->pts = mpegts_parse_pts(bitstream);
                    else
                        mpegts_debug_printf(NULL, "[TS] PTS_AND_DTS - ERROR - PTS header not correct\n");

                    //  next 4 bits should be '0001'
                    if (bits_get_next_bit(bitstream) == 0 && bits_get_next_bit(bitstream) == 0 && bits_get_next_bit(bitstream) == 0 && bits_get_next_bit(bitstream) == 1 )
                        p->dts = mpegts_parse_pts(bitstream);
                    else
                        mpegts_debug_printf(NULL, "[TS] PTS_AND_DTS - ERROR - DTS header not correct\n");


                    break;

                case NO_PTS : // '00'
                default:
//                    mpegts_debug_printf(NULL, "[TS] NO PTS or DTS\n");
                    break;
            }

            if (p->escr_flag)
            {
                mpegts_debug_printf(NULL, "PES_escr_flag\n");
                bits_get_bit_count(bitstream, 2 + 3 + 1 + 15 + 1 + 15 + 1 + 9 + 1, &rc);
            }

            if (p->es_rate_flag)
            {
                mpegts_debug_printf(NULL, "PES_es_rate_flag\n");
                bits_get_bit_count(bitstream, 1 + 22 + 1, &rc);
            }

            if (p->dsm_trick_mode_flag)
            {
                mpegts_debug_printf(NULL, "TRICK MODE -- BROKEN!!!!\n");
            }

            if (p->additional_copy_info_flag)
            {
                mpegts_debug_printf(NULL, "PES_additional copy_info\n");

                bits_get_bit_count(bitstream, 8, &rc);
            }

            if (p->pes_crc_flag)
            {
                mpegts_debug_printf(NULL, "PES_crc\n");
                bits_get_bit_count(bitstream, 16, &rc);
            }

            if (p->pes_extension_flag)
            {
                mpegts_debug_printf(NULL, "PES_EXT -- BROKEN!!!!\n");
            }

            if (p->pes_header_data_len)
            {
                uint16_t diff = p->pes_header_data_len - (bitstream->bit_offset / BYTE_SIZE - start);

                if (diff)
                {
                    uint8_t skip;
                    int q;

                    for (q = 0; q < diff; q++)
                    {
                        skip = (uint8_t) bits_get_bit_count(bitstream, BYTE_SIZE, &rc);
                        if (skip != MPEGTS_STUFF_BYTE)
                        {
                            mpegts_debug_printf(NULL, "[TS] Error skipping stuffing byte in PES header: %x\n", skip);
                            return NULL;
                        }
                    }
                }
            }
/*
			if (pmt_type)
			{
				_offset = bitstream->bit_offset;

				_type = mpegts_streamtype_valid(p->stream_id);
				switch (_type)
				{
					case MPEGTS_TYPE_VIDEO:
					{
						uint8_t *data = &bitstream->stream[(_offset/8)];
						if (data[0] == 0x00 && data[1] == 0x00 && data[2] == 0x01)
						{
							if (data[3] == 0x00)
							{
								uint16_t t_ref = ((uint16_t)data[4] << 2) | ((uint16_t)data[4] >> 6);
						        uint8_t picture_coding_type = (uint8_t) (data[5] & 0x38) >> 3;
									printf("Type, MPEG2? FrameType: %d, [P: %" PRId64", D: %" PRId64"]\n", picture_coding_type,  p->pts, p->dts);
//								printf("Type, VIDEO [0x%02x%02x%02x, %02x] [P: %" PRId64", D: %" PRId64"]\n", data[0], data[1], data[2], data[3], p->pts, p->dts);
							}
						}
					}
					break;

					case MPEGTS_TYPE_AUDIO:
						printf("Audio Not handled\n");
					break;
				}
				bitstream->bit_offset = _offset;
    	    }
*/
		}
    }
    return p;
}

void mpegts_output_writepts(uint8_t *data, uint64_t ipts, uint8_t flags)
{
	uint32_t i = 0;
	uint64_t p;
	ipts = ipts & 0xfffffffffLL; // clip to 36 bits 33 bits *8 before clipping
	data[i++] = (flags << 4) | (((ipts >> 30) & 0x07) << 1) | 1;
	p = (((ipts >> 15) & 0x7fff) << 1) | 1;
	data[i++] = p >> 8;
	data[i++] = p & 0xff;
	p = (((ipts) & 0x7fff) << 1) | 1;
	data[i++] = p >> 8;
	data[i++] = p & 0xff;
}

uint8_t mpegts_fix_pat(uint8_t *data, uint32_t len, uint16_t pmt_pid, uint16_t pgm_num)
{
	uint8_t ret = 0;
	if (data && len && pmt_pid && pgm_num)
	{
		uint8_t pid = ((data[1] & 0x1F) << 8) | data[2];
//		uint8_t ps = (data[1] & 0x40) >> 6;
        uint8_t adapt = (data[3] & 0x30) >> 4;

		if (pid == PAT_PID && data[4] == 0 && (!(adapt == ADAPT_FIELD_ONLY_NO_PAYLOAD || adapt == ADAPT_FIELD_AND_PAYLOAD)))
		{
			MPEGTS_NEWPAT_STRUCT *new_pat = NULL;
			new_pat = calloc(1, sizeof(MPEGTS_NEWPAT_STRUCT));

			if (new_pat)
			{
//				mpegts_debug_printf(NULL, "Fixing PAT, cont_count: %d, pgm: %d\n", cc, tuner->org_pmt->program_number);
				new_pat->ts_header = data;
				new_pat->ts_header_len = 5;
				new_pat->program_num = pgm_num;
				new_pat->pmt_pid = pmt_pid;
				new_pat->version = (data[10] & 0x3E) >> 1;
				new_pat->ts_id = (data[8] << 8) | data[9];
				mpegts_write_pat(new_pat);

				if (new_pat->new_pat_data)
				{
//					mpegts_debug_printf(NULL, "HERE1\n");
					memcpy(data, new_pat->new_pat_data, TS_PACKET_SIZE);
				}

				free(new_pat);
			}
		}
		else
		{
			ret = 1;
		}
	}
	return ret;
}


uint8_t mpegts_fix_pmt(uint8_t *data, uint32_t len,  uint16_t pcr_pid, uint16_t pgm_num, MPEGTS_MAP_ES *desc_es)
{
	uint8_t ret = 0;

	if (data && len && desc_es)
	{
		MPEGTS_NEWPMT_STRUCT *new_pmt = NULL;

		new_pmt = calloc(1, sizeof(MPEGTS_NEWPMT_STRUCT));

		if (new_pmt)
		{
			MPEGTS_MAP_ES *tmp_es = NULL, *del_es = NULL;
			new_pmt->program_number = pgm_num;
			new_pmt->pcr_pid = pcr_pid;
//			mpegts_debug_printf(NULL, "pgm: %d, pcr: %d\n", new_pmt->program_number, new_pmt->pcr_pid);
//			mpegts_debug_printf(NULL, ".. Start -- PMT\n");
			while (desc_es)
			{
				mpegts_debug_printf(NULL, "..PID: %d (0x%x) -- %d (0x%x)\n", desc_es->pid, desc_es->pid, desc_es->stream_type, desc_es->stream_type);
				switch (desc_es->stream_type)
				{
					case MPEGTS_STREAM_TYPE_SOMETHING:
					case MPEGTS_STREAM_TYPE_ISO13818_V: // MPEG2 Video
					case MPEGTS_STREAM_TYPE_ISO14496_10: // H264 Video
					case MPEGTS_STREAM_TYPE_ISO13818_3: // MP2 Audio
					case MPEGTS_STREAM_TYPE_ISO13818_7: // AAC Audio
					case MPEGTS_STREAM_TYPE_USER_DATA: // AC3 Audio (usually)
						tmp_es = desc_es;
					break;

					default:
						tmp_es = NULL;
					break;
				}

				if (tmp_es)
				{
					MPEGTS_MAP_ES * newes = calloc(1, sizeof(MPEGTS_MAP_ES));
//					mpegts_debug_printf(NULL, "BLAH: %d (0x%x)\n", tmp_es->pid, tmp_es->pid);

					if (newes)
					{
//						printf("\tAdding to PMT, PID: %d, Type: 0x%x (i_len: %d)\n", tmp_es->pid, tmp_es->stream_type, tmp_es->info_len);
						memcpy(newes, tmp_es, sizeof(MPEGTS_MAP_ES));
						newes->descriptor = NULL;
						newes->next = NULL;

						if (tmp_es->info_len)
						{
							newes->descriptor = calloc(1, tmp_es->info_len);
							memcpy(newes->descriptor, tmp_es->descriptor, tmp_es->info_len);
						}

//						mpegts_debug_printf(NULL, "Adding... %d\n", newes->pid);
						if (!new_pmt->desc_es)
						{
							new_pmt->desc_es = newes;
//							mpegts_debug_printf(NULL, "HI: %d %d\n", new_pmt->desc_es->pid, new_pmt->desc_es->stream_type);
					}
						else
						{
							tmp_es = new_pmt->desc_es;
							while(tmp_es->next)
								tmp_es = tmp_es->next;
							tmp_es->next = newes;
						}
					}
				}
				desc_es = desc_es->next;
			}
//			mpegts_debug_printf(NULL, ".. DONE -- PMT\n");

			new_pmt->ts_header = data;
			new_pmt->ts_header_len = 5;

			mpegts_write_pmt(new_pmt);

			if (new_pmt->new_pmt_data)
			{
//					mpegts_debug_printf(NULL, "HERE2\n");
				memcpy(data, new_pmt->new_pmt_data, TS_PACKET_SIZE);
			}

			tmp_es = new_pmt->desc_es;

			while (tmp_es)
			{
				del_es = tmp_es;
//				mpegts_debug_printf(NULL, "HERE: %d 0x%x -- len: %d\n", tmp_es->pid, tmp_es->stream_type, tmp_es->info_len);
				tmp_es = tmp_es->next;

				if (del_es->descriptor)
					free(del_es->descriptor);

				free(del_es);
			}

			free(new_pmt);
		}
	}
	return ret;
}

MPEGTS_PTS_DTS_RESTAMP * mpegts_offset_pts_dts(uint8_t *data, uint32_t len, uint64_t offset, uint16_t pmt_pid)
{
	MPEGTS_PTS_DTS_RESTAMP *ret = NULL;

	if (data && len && offset && pmt_pid)
	{
		BITSTREAM *bitstream = bits_create(data, len);
		if (bitstream)
		{
			MPEGTS_PACKET * packet = mpegts_parse_packet(bitstream);

			if (packet)
			{
				if (packet->adapt_field_control ==  ADAPT_FIELD_ONLY_NO_PAYLOAD || packet->adapt_field_control == ADAPT_FIELD_AND_PAYLOAD)
					packet->adaptation_field = mpegts_parse_adaptation_field(bitstream);

				if (packet->adapt_field_control == ADAPT_FIELD_AND_PAYLOAD || packet->adapt_field_control == ADAPT_NO_FIELD_ONLY_PAYLOAD)
				{
					if (!(packet->pid == PAT_PID || packet->pid == pmt_pid))
					{
						uint32_t write_offset = (bitstream->bit_offset/8) + 9;
						packet->payload_start_struct = mpegts_parse_payload_start(bitstream);

						if (packet->payload_start_struct)
						{
							MPEGTS_PAYLOAD_START *pusi = packet->payload_start_struct;
							uint64_t new_pts = 0, new_dts = 0;
							uint8_t tmpbits = 0, pts_dts_flags = PTS_ONLY, check_bits = PTS_ONLY;

							if (pusi->pts_dts_flag == PTS_AND_DTS)
							{
								check_bits = 0x03;
								pts_dts_flags = PTS_AND_DTS;
							}

							tmpbits = (data[write_offset] & 0xF0) >> 4;

							if (tmpbits == check_bits)
							{
								uint8_t _tmp1 = 0;
								ret = calloc(1, sizeof(MPEGTS_PTS_DTS_RESTAMP));
								ret->org_pts = new_pts = pusi->pts;
								ret->org_dts = new_dts = pusi->dts;

								if (offset > new_pts)
								{
									_tmp1 = 1;
									new_pts += MAX_33BITS;
								}
								new_pts -= offset;

								if (new_dts)
								{
									if (offset > new_dts)
										new_dts += MAX_33BITS;
									new_dts -= offset;
								}
								if (new_pts > offset)
								{
									ret->stamp_failed = 1;
									mpegts_debug_printf(NULL, "[%d] BAD PTS!!!!!!!!!!!!, org: %" PRId64" -- new: %" PRId64" -- off: %" PRId64" (%" PRId64")\n", _tmp1, pusi->pts, new_pts, offset, offset - pusi->pts);
								}

								if (new_dts && new_dts > offset)
								{
									mpegts_debug_printf(NULL, "BAD DTS!!!!!!!!!!!!\n");
									ret->stamp_failed = 1;
								}

								if (ret && !ret->stamp_failed)
								{
									ret->pts = new_pts;
									ret->dts = new_dts;

//									mpegts_debug_printf(NULL, "Restamp (pts: %" PRId64", dts: %" PRId64") Off: %" PRId64" (pts: %" PRId64", dts: %" PRId64") \n", pusi->pts, pusi->dts, offset, new_pts, new_dts);

									mpegts_output_writepts(data + write_offset, new_pts, pts_dts_flags);

									if (pts_dts_flags == PTS_AND_DTS)
										data[write_offset] |= 0x10;

									write_offset += 5;

									if (pts_dts_flags == PTS_AND_DTS)
									{
										pts_dts_flags = 0x01;
										mpegts_output_writepts(data + write_offset, new_dts, pts_dts_flags);
										write_offset += 5;
									}
								}

							}
						}
					}
				}
				mpegts_free_packet(packet);
			}
			free(bitstream);
		}
	}
	else
	{
		if (!offset)
			mpegts_debug_printf(NULL, "[MPEGTS] restamp: offset is 0\n");

		if (!pmt_pid)
			mpegts_debug_printf(NULL, "[MPEGTS] restamp: but pmt_pid is 0\n");
	}
	return ret;
}


uint8_t mpegts_streamtype_valid(uint8_t id)
{
	if (id >= MPEGTS_VIDEO_STREAM_START && id <= MPEGTS_VIDEO_STREAM_END)
		return MPEGTS_TYPE_VIDEO;

	if ((id >= MPEGTS_AUDIO_STREAM_START && id <= MPEGTS_AUDIO_STREAM_END) || (id == MPEGTS_PRIVATE_STREAM1))
		return MPEGTS_TYPE_AUDIO;

	mpegts_debug_printf(NULL, "HUH?\n");
	return MPEGTS_TYPE_UNHANDLED;
}

uint8_t mpegts_is_valid(char *v)
{
	uint8_t tmp_buf[1] = {0};

	int mpegts_file_handle = open(v, O_RDONLY);

	if (mpegts_file_handle)
	{
		int temp;
		temp = read(mpegts_file_handle, tmp_buf, sizeof(tmp_buf));
		close(mpegts_file_handle);

		if (temp != sizeof(tmp_buf) || tmp_buf[0] != TS_SYNC_BYTE)
			return 0;
//        else
//            mpegts_debug_printf(NULL, "[TS] First byte is valid\n");
	}
	return 1;
}

/*
int mpegts_get_ts_packets(int handle, uint8_t *data, uint32_t data_sz, uint32_t packet_count)
{
	if (handle && data && data_sz && packet_count)
	{
		if (data_sz >= packet_count * TS_PACKET_SIZE)
		{
			
		}
		else
		{
			return -1;
		}
	}
	return -2;
}
*/
//int get_bytes_from_handle(int handle, uint8_t * data, int len, long int s, long int us, uint32_t block)

/*
int32_t mpegts_need_sync(uint8_t *data, uint32_t data_sz)
{
	if (data && data_sz)
	{
		int i;
		for(i=0; i < data_sz; i+= TS_PACKET_SIZE)
		{
			if (data[i] != TS_SYNC_BYTE)
			{
				int j;
				for(j=i+1; j < data_sz - TS_PACKET_SIZE; j++)
				{
					
				}
				return 1;
			}
		}
		return 0;
	}
	return -1;
}
*/

/*
int8_t mpegts_get_frametype(uint8_t *data, uin32_t data_size)
{
	if (data)
	{
		int i;
		for (i = 0; i < TS_PACKET_SIZE - 5; i++)
		{
			if (data[i] == 0x00 && data[i+1] == 0x00 && data[i+2] == 0x01)
			{
//					mpegts_debug_printf(NULL, "es_type: 0x%x (%d)\n", tuner->video_es_type, tuner->video_es_type);
					if (tuner->video_es_type == MPEGTS_STREAM_TYPE_ISO13818_V || tuner->video_es_type == MPEGTS_STREAM_TYPE_SOMETHING)
//					if (tuner->video_es_type == MPEGTS_STREAM_TYPE_ISO13818_V)
					{
//							mpegts_debug_printf(NULL, "MPEG2 Video: 0x%x 0x%x 0x%x 0x%x\n", data[i], data[i+1], data[i+2], data[i+3]);
							if (data[i+3] == 0xB3)
							{
//								mpegts_debug_printf(NULL, "MPEG2 SEQ\n");
								ret = packet->frame_type = 1;
								break;
							}
							else if (data[i+3] == 0x00)
							{
								t_seq = (uint16_t)(data[i+4] << 2) | (data[i+5] >> 6);
								fr_type = (uint8_t)(data[i+5] & 0x38) >> 3;

//								mpegts_debug_printf(NULL, "MPEG2 FRAME: %s\n", MPEG2_PIC_TYPE[fr_type]);
								if (fr_type == 1)
								{
									ret = packet->frame_type = 1;
									break;
								}
							}
						}
						else if (tuner->video_es_type == MPEGTS_STREAM_TYPE_ISO14496_10)
						{
							mpegts_debug_printf(NULL, "H264 Video\n");
						}
					}
				}

*/
#ifdef _MAIN_



/*
                            uint16_t _t = MPEGTS_IO_IDX_HEADER;
                             _mpegts_do_fwrite(fileunit->idx_fhandle, (uint8_t*)&_t, sizeof(_t));
                             _mpegts_do_fwrite(fileunit->idx_fhandle, (uint8_t*)&_idx->packet_number, sizeof(_idx->packet_number));
                             _mpegts_do_fwrite(fileunit->idx_fhandle, (uint8_t*)&_idx->pts, sizeof(_idx->pts));
                             _mpegts_do_fwrite(fileunit->idx_fhandle, (uint8_t*)&_idx->dts, sizeof(_idx->dts));
*/


#define		IDX_BLOCK_SIZE		(TS_PACKET_SIZE * 500)




int main(int argc, char *argv[])
{
	if (argc > 1)
	{
		char *filename = argv[1];
		if (mpegts_is_valid(filename))
		{
			int fh = open(filename, O_RDONLY);

			mpegts_debug_printf(NULL, "TS is good and valid\n");

			if (fh > 2)
			{
				uint8_t *data = malloc(IDX_BLOCK_SIZE);
				off_t filesize = get_file_size(filename);
				off_t i, ret;
				int j;

				uint16_t pid;
				uint8_t start;
				uint64_t packet_no = 0, last_pts = 0, p_count = 0;

				mpegts_debug_printf(NULL, "Generate index for file: %s\n", filename);

				for(i = 0; i < filesize; i+= IDX_BLOCK_SIZE)
				{
					ret =  read(fh, data, IDX_BLOCK_SIZE);
					if (ret != IDX_BLOCK_SIZE)
						mpegts_debug_printf(NULL, "POOP\n");

					for(j=0; j < ret; j+= TS_PACKET_SIZE)
					{
						if (data[j] == TS_SYNC_BYTE)
						{
							pid = ((data[j+1] & 0x1F) << 8) | data[j+2];
							start = (data[j+1] & 0x40) >> 6;

							p_count++;

							if (start)
							{
								BITSTREAM *bitstream = bits_create(&data[j], TS_PACKET_SIZE);
								if (bitstream)
								{
									MPEGTS_PACKET *packet = mpegts_parse_packet2(bitstream, 0);
									if (packet)
									{

										if (packet->payload_start_struct)
										{
											if (packet->payload_start_struct->stream_id >= MPEGTS_VIDEO_STREAM_START &&
												packet->payload_start_struct->stream_id <= MPEGTS_VIDEO_STREAM_END)
											{
												if (packet->payload_start_struct->dts == 0)
												{
													if (packet->payload_start_struct->pts - last_pts > 90000)
													{
														packet_no = p_count;
														mpegts_debug_printf(NULL, "Video Start [%" PRId64"], PTS: %" PRId64" (%0.2f)\n", packet_no, packet->payload_start_struct->pts, packet->payload_start_struct->pts/90000.0);
														last_pts = packet->payload_start_struct->pts;
													}
												}
											}
										}
										mpegts_free_packet(packet);
									}
									free(bitstream);
								}

							}
						}
					}
				}

				close(fh);
				free(data);
			}
		}
		else
		{
			mpegts_debug_printf(NULL, "TS is NOT good and valid\n");
		}
	}
	return 0;
}
#endif

